#include <Arduino.h>
#include <Wire.h>

#define DEBUG_MODE 1          // 调试模式开关
#define PCA9685_ADDR 0x40     // PCA9685地址
#define SERVO_MIN_PULSE 500   // 0度脉冲宽度（微秒）
#define SERVO_MAX_PULSE 2500  // 180度脉冲宽度（微秒）
#define UPDATE_INTERVAL 15    // 15ms更新周期（约66.6Hz）
#define RISE_STEP 10          // 上升步长（0→90）
#define FALL_STEP 5           // 下降步长（90→0）

// 函数声明
void setupPCA9685(uint8_t addr);
void setPWMFreq(uint8_t addr, float freq);
void setServoAngle(uint8_t addr, uint8_t channel, uint8_t angle);

void setup() {
  #if DEBUG_MODE
  Serial.begin(115200);
  delay(1000);
  #endif

  Wire.begin(21, 22);        // SDA=21, SCL=22
  Wire.setClock(400000);     // 提升I2C时钟到400kHz
  
  setupPCA9685(PCA9685_ADDR);
  
  #if DEBUG_MODE
  Serial.println("系统初始化完成");
  #endif
}

void loop() {
  static unsigned long lastUpdate = 0;
  static int16_t currentAngle = 0;  // 当前角度（有符号）
  static bool isRising = true;      // 运动方向标志

  // 定时控制
  if(millis() - lastUpdate < UPDATE_INTERVAL) return;
  lastUpdate = millis();

  // 角度更新逻辑
  if(isRising) {
    currentAngle += RISE_STEP;
    if(currentAngle >= 90) {
      currentAngle = 90;
      isRising = false;
      delay(50);  // 顶端短暂停留
    }
  } else {
    currentAngle -= FALL_STEP;
    if(currentAngle <= 0) {
      currentAngle = 0;
      isRising = true;
      delay(150); // 底端稳定时间
    }
  }

  // 设置所有通道
  for(uint8_t ch=0; ch<16; ch++) {
    setServoAngle(PCA9685_ADDR, ch, (uint8_t)currentAngle);
  }

  #if DEBUG_MODE
  static int16_t lastAngle = -1;
  if(currentAngle != lastAngle) {
    Serial.print("当前角度: ");
    Serial.print(currentAngle);
    Serial.println("°");
    lastAngle = currentAngle;
  }
  #endif
}

// PCA9685初始化（保持原有实现）
void setupPCA9685(uint8_t addr) {
  Wire.beginTransmission(addr);
  Wire.write(0x00); // MODE1
  Wire.write(0x20); // AI使能
  Wire.endTransmission();

  Wire.beginTransmission(addr);
  Wire.write(0x01); // MODE2
  Wire.write(0x04); // OUTDRV推挽输出
  Wire.endTransmission();

  setPWMFreq(addr, 50);
  delay(50);

  Wire.beginTransmission(addr);
  Wire.write(0xFA); // ALL_LED_ON_L
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.endTransmission();
}

// PWM频率设置（保持原有实现）
void setPWMFreq(uint8_t addr, float freq) {
  freq = constrain(freq, 24, 1526);
  uint8_t prescale = round(25000000.0 / (4096 * freq)) - 1;

  Wire.beginTransmission(addr);
  Wire.write(0x00);
  Wire.write(0x10);
  Wire.endTransmission();

  Wire.beginTransmission(addr);
  Wire.write(0xFE);
  Wire.write(prescale);
  Wire.endTransmission();

  Wire.beginTransmission(addr);
  Wire.write(0x00);
  Wire.write(0x20);
  Wire.endTransmission();
}

// 优化后的舵机控制
void setServoAngle(uint8_t addr, uint8_t channel, uint8_t angle) {
  // 硬件级消抖处理
  static uint16_t lastTicks = 0;
  angle = constrain(angle, 0, 90);  // 限制在0-90度范围
  
  // 脉冲计算优化（整数运算）
  uint16_t pulse = SERVO_MIN_PULSE + (angle * 2000UL) / 90;
  uint16_t ticks = (pulse * 4096UL) / 20000;

  // 0度位置稳定处理
  if(angle == 0) {
    ticks = (SERVO_MIN_PULSE * 4096UL) / 20000;
  }

  Wire.beginTransmission(addr);
  Wire.write(0x06 + 4 * channel);
  Wire.write(lowByte(0));
  Wire.write(highByte(0));
  Wire.write(lowByte(ticks));
  Wire.write(highByte(ticks) & 0x0F);
  Wire.endTransmission();
}